/**********************************************************************
 Copyright (c) 2020-2024, Unitree Robotics.Co.Ltd. All rights reserved.
***********************************************************************/

#pragma once

#define BOOST_BIND_NO_PLACEHOLDERS

#include <memory>
#include <iostream>
#include <string>
#include <stdio.h>

#include <iterator>
#include <algorithm>
#include <chrono>
#include <vector>
#include <mutex>
#include <deque>
#include <thread> // For std::this_thread::sleep_for (可选)

// PCL core and I/O
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/common/transforms.h>
#include <pcl/common/common.h>
#include <pcl/filters/passthrough.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/search/kdtree.h>
#include <pcl/point_cloud.h>
#include <pcl/common/centroid.h>    
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/filters/extract_indices.h>


// Eigen
#include <Eigen/Dense>
#include <Eigen/Geometry>

// ROS messages
#include "sensor_msgs/msg/point_cloud2.hpp"
#include "sensor_msgs/msg/imu.hpp"
#include "std_msgs/msg/header.hpp"
#include "visualization_msgs/msg/marker.hpp"
#include "geometry_msgs/msg/transform_stamped.hpp"
#include "geometry_msgs/msg/point.hpp"
#include "geometry_msgs/msg/pose_array.hpp"
#include "nav_msgs/msg/odometry.hpp" // 新增：里程计消息，用于动态TF


// ROS utilities
#include "rclcpp/rclcpp.hpp"
#include "rclcpp/time.hpp"
#include "rclcpp/duration.hpp"
#include "pcl_conversions/pcl_conversions.h"

// TF2
#include "tf2_ros/transform_broadcaster.h"
#include "tf2_ros/buffer.h"
#include "tf2_ros/transform_listener.h"
#include "tf2/LinearMath/Quaternion.h"
#include <tf2_eigen/tf2_eigen.hpp>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>


// SDK
#include "unitree_lidar_sdk_pcl.h" // Defines PointType
#include "unitree_lidar_protocol.h"

// OpenCV
#include <opencv2/opencv.hpp>
#include <geometry_msgs/msg/pose_array.hpp>
#include <cv_bridge/cv_bridge.h>

// ONNX Runtime (替代Torch)
#include <onnxruntime_cxx_api.h>
#include <unistd.h> 
#include "sensor_msgs/msg/image.hpp"

#include <sstream>
#include <iomanip>
#include <limits>
#include <cmath>
#include <vector>        // 添加必要的头文件
#include <algorithm>  
#include <Eigen/SVD>
#include <tf2/LinearMath/Quaternion.h>
#include <geometry_msgs/msg/transform_stamped.hpp>

using std::placeholders::_1;



class UnitreeLidarSDKNode : public rclcpp::Node
{
public:
    explicit UnitreeLidarSDKNode(const rclcpp::NodeOptions &options = rclcpp::NodeOptions());
    ~UnitreeLidarSDKNode(); // 添加析构函数以清理资源
    void timer_callback();

    // 定义结构体存储障碍物信息（确保在 vector 和 lambda 之前定义）
    struct ObstacleInfo {
        float distance;           // 障碍物距离
        float width;              // 宽度
        float height;             // 高度
        float length;             // 长度
        float left_distance;      // 障碍物相对于雷达的左侧距离
        float right_distance;     // 障碍物相对于雷达的右侧距离
        Eigen::Vector4f centroid; // 质心，用于坐标系转换
    };



private:
    // ********************** 新增TF转换逻辑 ********************* //
    std::shared_ptr<tf2_ros::Buffer> tf_buffer_;              // TF缓冲区
    std::shared_ptr<tf2_ros::TransformListener> tf_listener_; // TF监听器
    // ***************************end**************************** //

    // ********************** 新增点云多帧积累 ********************* //
    std::deque<pcl::PointCloud<PointType>::Ptr> cloud_buffer_;
    size_t kBufferSize; // 修改为可调参数
    // ***************************end**************************** //

    void processMergedPointCloud(pcl::PointCloud<PointType>::Ptr merged, rclcpp::Time timestamp);

    // // 新增私有函数，用于模块化处理
    // void processPointCloud(const PointCloudUnitree& cloud, pcl::PointCloud<PointType>::Ptr& cloudOut);
    // void filterAndClusterPointCloud(pcl::PointCloud<PointType>::Ptr& cloud, rclcpp::Time timestamp);

protected:
    // ROS发布器
    rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub_cloud_;           // 原始点云
    rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub_filtered_cloud_; // 过滤后的点云
    rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr pub_imu_;                     // IMU数据
    rclcpp::Publisher<geometry_msgs::msg::PoseArray>::SharedPtr pub_obstacles_;      // 障碍物位置
    rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr pub_marker_;       // 障碍物边界框可视化
    rclcpp::TimerBase::SharedPtr timer_;
    std::shared_ptr<tf2_ros::TransformBroadcaster> broadcaster_;

    // Unitree Lidar Reader
    UnitreeLidarReader *lsdk_;

    // Config params
    int work_mode_;
    int initialize_type_;
    int local_port_;
    std::string local_ip_;
    int lidar_port_;
    std::string lidar_ip_;
    std::string serial_port_;
    int baudrate_;
    
    int cloud_scan_num_;
    bool use_system_timestamp_;
    double range_min_;
    double range_max_;

    std::string cloud_frame_;
    std::string cloud_topic_;

    std::string imu_frame_;
    std::string imu_topic_;

    // 过滤参数
    double filtered_cloud_z_min_; // 最小高度
    double filtered_cloud_z_max_; // 最大高度
    double obs_max_detection_range_; // 最大检测距离
    double obs_min_detection_range_xy_; // 最小检测距离（XY平面）
    double obs_horizontal_fov_deg_; // 水平视野角度

        // 新增：用于PCL算法的可调参数
    double ground_segmentation_distance_threshold_;
    int statistical_outlier_removal_mean_k_;
    double statistical_outlier_removal_stddev_thresh_;
    double euclidean_cluster_tolerance_;
    int euclidean_cluster_min_size_;
    int euclidean_cluster_max_size_;


};
